/*
 * SUDSC2Interface.h
 *
 *  Created on: June 21, 2011
 *      Author: shinobiboriken
 *      Editor: ErbsNSpices
 */

#include "SUDSC2Intf.h"

#include <string.h>
#include <stdlib.h>
#include <stdio.h>
#include <unistd.h>
#include <fcntl.h>
#include <termios.h>
#include <stdint.h>
# include <getopt.h>
# include <time.h>
# include <errno.h>
# include <iostream>
using namespace std;

SUDSC2Intf::SUDSC2Intf()
{
	//Set up the file-write connection
	//Read in first receive (garbage) and reset the signal upon initialization
	Initialize();
}

void SUDSC2Intf::Initialize()
{
	resetWriter = open("/sys/class/gpio/gpio161/value", O_RDWR); //161
	leftDetReader = open("/sys/class/gpio/gpio164/value", O_RDWR); //164
	rightDetReader = open("/sys/class/gpio/gpio165/value", O_RDWR); //165

	//Read first time garbage
	int q;
	int leftStatus = 0;
	int rightStatus = 0;
	char readwriteBuf[2];
	leftStatus = read(leftDetReader, readwriteBuf, 2);
//	cout << (int)readwriteBuf[0] << endl;
	rightStatus = read(rightDetReader, readwriteBuf, 2);
//	cout << (int)readwriteBuf[0] << endl;

	//Reset flip flop
	readwriteBuf[0] = '1';
	q=write(resetWriter,readwriteBuf,1);
	readwriteBuf[0] = '0';
	q=write(resetWriter,readwriteBuf,1);
}

//Call me from C2
//0 = nothing, 1 = left detection, 2 = right detection, 3 = both
int SUDSC2Intf::checkDetections() {
	//Specific sensor status
	int q;
	int leftStatus = 0;
	int rightStatus = 0;
	char readwriteBuf[2];

	//Open the read connections (doesn't work from separate function
	leftDetReader = open("/sys/class/gpio/gpio164/value", O_RDWR); //164
	rightDetReader = open("/sys/class/gpio/gpio165/value", O_RDWR); //165
//	cout << resetWriter << endl;
//	cout << leftDetReader << endl;
//	cout << rightDetReader << endl;

	//Check left sensor for detection
	q = read(leftDetReader, readwriteBuf, 2);
	if(q == -1)
	{
//		cout << "SUDS Left Read Failed" << endl;
	}
	else
	{
//		cout << (int)readwriteBuf[0] << endl;
		if(readwriteBuf[0] == '1')
			leftStatus = 1;
	}

	//Check right sensor for detection
	q = read(rightDetReader, readwriteBuf, 2);
	if(q == -1)
	{
//		cout << "SUDS Right Read Failed" << endl;
	}
	else
	{
//		cout << (int)readwriteBuf[0] << endl;
		if(readwriteBuf[0] == '1')
			rightStatus = 2;
	}

	//Reset flip flop
	readwriteBuf[0] = '1';
	q=write(resetWriter,readwriteBuf,1);
	readwriteBuf[0] = '0';
	q=write(resetWriter,readwriteBuf,1);

	return (leftStatus + rightStatus);
}

//int main()
//{
//	int i;
//	int o;
//	SUDSC2Intf* sci = new SUDSC2Intf();
//
//	while(1)
//	{
//		cout << "Type 1 to read data, 2 to quit" << endl;
//		cin >> i;
//		if (i == 1)
//		{
//			o = sci -> checkDetections();
//			cout << (int)o << endl;
//		}
//		else
//			break;
//	}
//	return 0;
//}
